#define _Mn 10000
#define _Mv 10000
#define _TAU 0.57

template<class Resample>
class PF_Thurstone{
	public:
	PF_Thurstone(imat X, Resample *R)
	{
		_X=X;
		_R=R;
		_p=_X.n_cols;
		_n=X.n_rows;
		boost::random::normal_distribution<> N2;
		rnorm= new RandomG::Random<boost::random::normal_distribution<> >(N2);
		boost::random::uniform_01<> U;
		_runif=new RandomG::Random<boost::random::uniform_01<> >(U);
		_d=new boost::math::normal_distribution<>(0,1); 
		alpha=1;
		beta=1;
		mat B(_p,_p);
		B.fill(1);
		_B=diagmat(B);
		mat e(1,_p);
		e.fill(1);
		_e=e;
		_deriv_m=e;
		B.fill(1);
		_Mm=diagmat(B);
		_Ms=1;
		_I=_Mm;
		_seps=1;
	}

	double PF(mat m, double s, imat x, int Mx, int bb)
	{
		double thres=0.9*Mx;
		mat z(Mx,_p);
		z.fill(0);
		if(bb==0) 
		{
			_deriv_Sig=0;
			for(int k=0;k<_p;k++)
			{
				_deriv_m(0,k)=-m(0,k)/_B(k,k);
			}
			_deriv_Sig=(-alpha-1)/s+beta/(s*s);
		}

		int k1=Searchindex(x,1);
		double *w=new double[Mx];
		double cd,arg,u;
		//cout << "k1 " << k1 << "\n"; 
		for(int i=0;i<Mx;i++)
		{
			z(i,k1)=(*rnorm)()*sqrt(s)+m(0,k1);
			w[i]=1;
		}
		double res=0;
		int k_1=k1;
		for(int t=1;t<_p;t++)
		{
			//Resample
			double ess=ESS(w,Mx);
			if(ess<thres){
			//	cout << "ess "<< ess<< " " ;
				res+=fmean(w,Mx);
				mat Z=z(span::all,k_1);
				fResample(Mx,w,&Z);
				z(span::all,k_1)=Z;	
			}
			//Move
			k1=Searchindex(x,t+1);
			//cout << "i " << t+1<< "k1 " << k1 << "\n"; 
			double tm=m(0,k1);
			double s2=sqrt(s);
			for(int i=0;i<Mx;i++)
			{
			//	cout << "sk1 " << s(0,k1);
				cd=cdf(*_d,(z(i,k_1)-tm)/s2);
				u = (*_runif)(); 
				arg=u*cd;
				if(arg > .999999999) arg=.999999999;
				 if(arg < .0000000001) arg=.0000000001;
				z(i,k1)=tm+quantile(*_d,arg)*s2;
				//cout <<" " <<z(i,k1);
				w[i]*=cd;
			}
			k_1=k1;

		}
		res+=fmean(w,Mx);
		double deriv_Sig;//=cov(z,0);
		deriv_Sig=0;
		mat temp;
		mat sum1(1,_p);
		sum1.fill(0);
		double normc=0;
		for(int i=0;i<Mx;i++)
		{
			sum1+=w[i]*z(i,span::all);
			normc+=w[i];
			temp=z(i,span::all)-m;
		//	cout << "z " <<  z(i,span::all);
			deriv_Sig+=w[i]*as_scalar(sum(temp%temp,1));
		//	cout << "tt " << (temp.t()*temp);
		}
		mat deriv_m=sum1/normc-m;
		deriv_Sig=deriv_Sig/(2*s*s*Mx)-_p/(2*s);
		//cout << "dS" << deriv_Sig;

		mat Sigg(_p,_p);
		Sigg.fill(0);
		for(int i=0;i<_p;i++)
		{
			deriv_m(0,i)=deriv_m(0,i)/s;
		}
		#pragma omp critical
		{
			_deriv_m+=deriv_m;
	//		cout << deriv_Sig;
		//	cout << Sigg;
			_deriv_Sig+=deriv_Sig;
		}	
		delete[] w;
		return res;
	}
	double ESS(double *w, int M)
	{
	
		double sum=0;
		for(int i=0;i<M;i++)
		{
			sum+=w[i];
		}
		double sum2=0;
		for(int i=0;i<M;i++)
		{
			sum2+=pow(w[i]/sum,2);
		}
		return (double)1/sum2;
	}

	void fResample(int Mx, double *w, mat *z)
	{
		mat v=growingvect(Mx);
		(*_R)(&Mx,w,v);
		Arangemat(*z,v);
		for(int i=0;i<Mx;i++)
		{
			w[i]=1;
		}
	}

		vector<mat> operator()(int Mt, int Mx, mat im, double is)
		{
			mat m(Mt,_p);
			mat s(Mt,1);
			m.fill(0);
		//	for(int i=0;i<_p;i++) m(0,i)=-(double)_X(0,i)/_p+1;
			//cout << "m " << m;
			//m(0,0)=0.1;
			m(0,span::all)=im;
			s(0,0)=is;
			mmn=m(0,span::all);
			msn=mmn;
			mat V1(_p,_p);
			int kk=0;
			mat V2(_p,_p);
			V1.fill(1);
			V2.fill(1);
			V1=diagmat(V1);
			V2=diagmat(V2);
			_eps1=0.05;
			_eps2=0.01;
			mat m0(_p,1);
			m0.fill(0);
			Distribution::Gaussian G1(_p,m0,V1);
			Distribution::Gaussian G2(_p,m0,V2);

			double post=logPrior(m(0,span::all),s(0,0));
			#pragma omp parallel 
			{
				#pragma omp reduction(+:post)
				for(int j=0;j<_n;j++)
				{
					post=post+PF(m(0,span::all),s(0,0),_X(j,span::all),Mx,j);
					//cout << "post " << exp(post) << "\n";
				}
			}
			_deriv_mt=_deriv_m;
			_deriv_Sigt=_deriv_Sig;	
			double lalpha;

			for(int i=1;i<Mt;i++)
			{
				_gamman=(double)0.001/(pow(i,0.8));
			//	cout << "_Mm " <<_Mm;
			//	cout << "_Ms " <<_Mm;
			//	_Mm=_Mm+_seps*_I;
			//	_Ms=_Ms+_seps*_I;
				mat t1=chol(_Mm).t()*G1.r(1);
				double t2=sqrt(_Ms)*(*rnorm)();
			//	cout << chol(_Mm);
				mat meanm=0.5*_eps1*_eps1*Bound(_deriv_mt,(double)10000)*_Mm;
				
			//	cout << "Mm" << _Mm; 
			//	cout << "mm " << Bound(_deriv_mt,100);

				mat prop_m=m(i-1,span::all)+meanm+_eps1*t1.t();
				double mean_s=0.5*_eps2*_eps2*min(_deriv_Sigt,(double)10000)*_Ms;
				//cout << "propm " << prop_m;
				//cout << " actual " << m(i-1,span::all);
				double prop_s=s(i-1,0)+mean_s+_eps2*t2;
				cout << "props " << prop_s << "\n";
				if(prop_s<0)
				{
					m(i,span::all)=m(i-1,span::all);
					s(i,0)=s(i-1,0);
				}else{
					double target=logPrior(prop_m,prop_s);
					for(int j=0;j<_n;j++)
					{
						target+=PF(prop_m,prop_s,_X(j,span::all),Mx,j);
					//	cout << "t"  <<target << " ";
					}
				//	double propf=Gd(prop_m,prop_s,meanm,mean_s,_eps1,_eps2);
					mat meanmn=0.5*_eps1*_eps1*Bound(_deriv_m,(double)10000)*_Mm;
					double meansn=0.5*_eps2*_eps2*min(_deriv_Sig,(double)10000)*_Ms;
					double propf=Gauss(prop_m,m(i-1,span::all)+meanm,_eps1*_eps1*_Mm)+log(pdf(*_d,(prop_s-s(i-1,0)-mean_s)/(_eps2*sqrt(_Ms)))/(_eps2*sqrt(_Ms)));
					cout << "meanmn " <<meanmn;
				//	cout << "meanm " <<meanm;
					cout << "propf " << (prop_s-s(i-1,0)-mean_s);//log(pdf(*_d,(prop_s-s(i-1,0)-mean_s)/(_eps2*sqrt(_Ms)))/(_eps2*sqrt(_Ms)));//propf;
					cout << "post " << post;
					cout << "target " << target;
				//	double propp=Gd(m(i-1,span::all),s(i-1,span::all),meanm,mean_s,_eps1,_eps2);
					double propp=Gauss(m(i-1,span::all),prop_m+meanmn,_eps1*_eps1*_Mm)+log(pdf(*_d,(s(i-1,0)-prop_s-meansn)/(_eps2*sqrt(_Ms)))/(_eps2*sqrt(_Ms)));
					cout << "propp " << (s(i-1,0)-prop_s-meansn);//log(pdf(*_d,(s(i-1,0)-prop_s-meansn)/(_eps2*sqrt(_Ms)))/(_eps2*sqrt(_Ms)));//propp;
					double u=log((*_runif)());
					lalpha = min((target-propf-post+propp),(double)0);
					//cout<< "m " << meanmn<< "\n";
					cout << "s " <<  meansn <<"\n";
					cout << "s " <<  mean_s <<"\n";
					if(u<lalpha)
					{
						post=target;
						m(i,span::all)=prop_m;
						s(i,0)=prop_s;
						_deriv_mt=_deriv_m;
						_deriv_Sigt=_deriv_Sig;
						kk++;

					}else{
						m(i,span::all)=m(i-1,span::all);
						s(i,0)=s(i-1,0);
					}
				}
			
				mmn=p32(mmn+_gamman*(m(i,span::all)-mmn),_Mn);
			//	msn=p32(msn+_gamman*(s(i,span::all)-msn),_Mn);
				_Mm=p32(_Mm+_gamman*((m(i,span::all)-mmn).t()*(m(i,span::all)-mmn)-_Mm),_Mv);
				//_Ms=p32(_Ms+_gamman*((s(i,span::all)-msn).t()*(s(i,span::all)-msn)-_Ms),_Mv);
			
				
				double alpha=exp(lalpha);
				if(alpha!=alpha) alpha=0;
				
				_eps1=proj1(_eps1+_gamman*(alpha-_TAU),1);
				_eps2=proj1(_eps2+_gamman*(alpha-_TAU),1);
				cout << "#"<< (double)kk/i << " "<< lalpha << "eps "<< _eps1 << "\n";
			}
			vector<mat> res;
			res.push_back(m);
			res.push_back(s);
			
			return res;		
		}

		double fmean(double *w, int Mx){
			double sum=log(w[0]);
			for(int i=0;i<Mx;i++)
			{
				if(w[i]!=0)
				{
					sum=log_add(log(w[i]),sum);
				}
			}
			return sum-log(Mx);
					
		}
		double logPrior(mat m, double s)
		{
			double res=0;
			for(int i=0;i<_p;i++)
			{
				res+=log(pdf(*_d,m(0,i)/sqrt(_B(i,i)))/sqrt(_B(i,i)));
			}
				res+=log(s)*(-alpha-1)-beta/s;
			return res;
		}
		int Searchindex(imat x,int k)
		{
			int l=-1;
			for(int i=0;i<_p;i++)
			{
				if(x(0,i)==k) l=i;
			}
			return l;
		}
		int is0(mat x)
		{
			int res=0;
			for(int i=0;i<_p;i++)
			{
				if(x(0,i)<=0) res=1;
			}
			return res;
		}

		double Gd(mat x, mat y, mat mx, mat my, double eps1, double eps2)
		{
			double sum=0;
			for(int i=0;i<_p;i++)
			{
				sum+=log(pdf(*_d,(x(0,i)-mx(0,i))/eps1)/eps1);			
				sum+=log(pdf(*_d,(y(0,i)-my(0,i))/eps2)/eps2);			
			}
			return sum;
		}
		mat Bound(mat x, double delta)
		{
			double nm=norm(x.t());
			if(delta< nm) x=(x/nm)*delta;
			//x.fill(0);
			return x;
		}
		double Gauss(mat x, mat m, mat Sig)
		{	
			double res=0;
			mat foo=x-m;
			double bar=as_scalar(foo*inv(Sig)*foo.t());
			//cout << "bar" << bar;
			res=-_p*0.5*log(2*PI)-0.5*log(det(Sig))-0.5*bar;
			//cout << "res " << log(det(Sig));
		//	if(log(det(Sig))<-10000) res=-10000;
			return res;
		}
		mat p32(mat X, double delta)
		{
			double nm=sqrt(as_scalar(trace(X*X.t())));
			if(nm>delta) X=delta/nm*X;
			return X;
		}

		double proj1(double x, double delta)
		{
			if(x>delta) { x=delta;}
			if(x<0){ x=0.0000000000000000000000000001;}
			return x;
		}


	private:
		imat _X;
		Resample *_R;
		int _p;
		double alpha;
		double beta;
		mat _B;
		int _n;
		double _eps1;
		double _eps2;
		RandomG::Random<boost::random::normal_distribution<> > *rnorm;
		RandomG::Random<boost::random::uniform_01<> > *_runif;
		boost::math::normal_distribution<> *_d;
		mat _deriv_m;
		mat _deriv_mt;
		double _deriv_Sigt;
		double _deriv_Sig;
		mat _e;
		mat _Mm;
		double _Ms;
		double _gamman;
		mat mmn;//mean of first vect
		mat msn;//second
		mat _I;
		double _seps;

};

